Hierarchical Motion Planning with Kinodynamic Feasibility Guarantees

Motion planning for mobile vehicles involves the solution of two disparate sub-problems: the satisfaction of highlevel logical task specifications and the design of low-level vehicle control laws. A hierarchical solution of these two sub-problems is efficient, but may not ensure compatibility between the highlevel planner and the dynamic constraints of the vehicle. To guarantee such compatibility, we propose a motion planning framework based on a special interaction between these two levels of planning. In particular, we solve a special shortest path problem on a graph at the higher level of planning, and we use the lower level planner to determine the costs of the paths in that graph. The overall approach hinges on two novel ingredients: a graph-search algorithm that operates on sequences of nodes, and a lower-level planner that ensures consistency between the two levels of hierarchy, by providing meaningful costs for the edge transitions of the higher level planner using dynamically feasible, collision-free trajectories.